#include "rtp.h"
#include "lrtsp_typedef.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <netinet/in.h>
#include "lrtsp_stream.h"


#define READ_LEN 960000
#define RTP_SEND_BUFFER_SIZE		2048	
#define SLICE_SIZE 1448
#define SLICE_FSIZE 1435
#define NALU_BUFFER_SIZE 1448

typedef struct _RTP_header {
	/* byte 0 */
	#ifdef WORDS_BIGENDIAN
		unsigned char version:2;
		unsigned char padding:1;
		unsigned char extension:1;
		unsigned char csrc_len:4;
	#else
		unsigned char csrc_len:4;		
		unsigned char extension:1;		
		unsigned char padding:1;		
		unsigned char version:2;	
	#endif
	/* byte 1 */
	#if WORDS_BIGENDIAN
		unsigned char marker:1;
		unsigned char payload:7;		
	#else
		unsigned char payload:7;
		unsigned char marker:1; 
	#endif
	/* bytes 2, 3 */
	unsigned short seq_no;
	/* bytes 4-7 */
	unsigned int timestamp;
	/* bytes 8-11 */
	unsigned int ssrc;					/* stream number is used here. */
} RTP_header;


ssize_t write_n(int fd, const void *vptr, size_t n, int is_udp)
{
	size_t		nleft;
	ssize_t		nwritten;
	const char	*ptr;

	ptr = vptr;
	nleft = n;

	if(is_udp){
        while (nleft > 0) {
            nwritten = write(fd, ptr, nleft);
            
            if ( nwritten <= 0) {
                if (errno == EINTR)
                    nwritten = 0;       /**** and call write() again ****/
                else
                    return(-1);         /**** error ****/
            }
        
            nleft -= nwritten;
            ptr   += nwritten;
        }
	}
	else{
        while (nleft > 0) {
            nwritten = send(fd, ptr, nleft, 0);
            if ( nwritten <= 0) {
                if(errno == EINTR || errno == EAGAIN){
                    printf("rtsp send errno == EINTR || errno == EAGAIN\n");
                    nwritten = 0; /**** and call send() again ****/
                    return -1;
                }
                else{
                    perror("rtp send perror\n");
                    return -1;
                }
            }
        
            nleft -= nwritten;
            ptr   += nwritten;
        }

	}

	return(n);
}


int udp_write(unsigned char* nalu_buffer, int len, struct RtspClient* rtsp_client)
{
	int result;
	unsigned char rtp_send_buffer[RTP_SEND_BUFFER_SIZE];
	if(len>(RTP_SEND_BUFFER_SIZE+4)) return 0;
	
	if(rtsp_client_get_transport_mode(rtsp_client) == RTSP_TRANSPORT_TCP){
		memset(rtp_send_buffer, 0, RTP_SEND_BUFFER_SIZE);
		rtp_send_buffer[0] = 0x24;
		rtp_send_buffer[1] = 0;
        rtp_send_buffer[2] = ( len >> 8 ) & 0xff;
        rtp_send_buffer[3] = len & 0xff;
		memcpy(rtp_send_buffer + 4, nalu_buffer, len);
	}
	
	again:
	if(rtsp_client_get_transport_mode(rtsp_client) == RTSP_TRANSPORT_TCP){
		result = write_n(rtsp_client_get_fd(rtsp_client), rtp_send_buffer, len + 4, 0);
        if(result < 0){
            lrtsp_stream_remove_rtsp_client(rtsp_client);
            return -1;
        }
	}
	else{
		result=write_n(rtsp_client_get_fd(rtsp_client), rtp_send_buffer, len, 1);
	}
	if(result <= 0){
		goto again;
	}
	else{
		//usleep(DE_TIME);
	}
	return 0;
}

int get_frame_size(unsigned char *inbuf, int inbuffer_data_len, unsigned char *outbuf)
{
	int first_find = 0,size = 0;
	unsigned char *p1,*p2;
	int count = 0;
	
	p1=&inbuf[0];
	p2=inbuf;
	
	while(1){
		if((p2[0]==0x00)&&(p2[1]==0x00)&&(p2[2]==0x00)&&(p2[3]==0x01)){
			first_find++;
			if(first_find==2){
				goto retu;
			}
			p2+=4;
			count+=4;
		}
		else{
			p2++;
		}
    	count++;
		if(count >= inbuffer_data_len){
            goto retu;
		}
	}

retu:
	if(first_find == 2){
		size = p2 - p1;
	}
	else{
		size = inbuffer_data_len;
	}

    if(size > READ_LEN){
        size = READ_LEN;
    }

    memcpy(outbuf,inbuf,size);
	
	return size;	
}

int _check_start_code(unsigned char *inbuf)
{
  	if((inbuf[0]==0x00)&&(inbuf[1]==0x00)&&(inbuf[2]==0x00)&&(inbuf[3]==0x01)){
		return 1;		
	}
	
    printf("Func(%s) Line(%d): check failed  \n", __func__, __LINE__);
    
	return 0;
}

int build_rtp_header(RTP_header *r, struct RtspClient* rtsp_client)
{	
	r->version = 2;
	r->padding = 0;
	r->extension = 0;
	r->csrc_len = 0;
	r->marker=0;
	r->payload = 96;   /*video*/
//	r->payload= (rtsp_client->get_avdata_type == 0)?96:8;
//	r->seq_no=htons(rtsp_client->cmd_port.seq);
	r->seq_no = rtsp_client_get_seq(rtsp_client);
//	rtsp_client->cmd_port.timestamp += 3600;
	r->timestamp=htonl(rtsp_client_get_timestamp(rtsp_client));
//	r->timestamp=htonl(rtsp_client->cmd_port.timestamp);
	r->ssrc = htonl(rtsp_client_get_ssrc(rtsp_client));
//	r->ssrc = htonl(rtsp_client->cmd_port.ssrc);
	
	return 0;
}


int build_signal_packet(unsigned char *inbuffer,int frame_size,int nalu_number,struct RtspClient* rtsp_client)
{
	int len;
	RTP_header r;	   /**** 12 bytes ****/
    int seq = rtsp_client_get_seq(rtsp_client);
	unsigned char nalu_buffer[NALU_BUFFER_SIZE];
	int ret = 0;

	inbuffer+=4;
	build_rtp_header(&r,rtsp_client);
//	r.seq_no=htons(rtsp_client->cmd_port.seq++);
	r.seq_no = htons(rtsp_client_get_seq(rtsp_client));
	rtsp_client_set_seq(rtsp_client, ++seq);
	r.marker=1;
	memcpy(nalu_buffer,&r,sizeof(r));
	memcpy(nalu_buffer+12,inbuffer,(frame_size-4));
	len=(frame_size-4)+12;
	/****  write buff to client   ****/
	ret = udp_write(nalu_buffer, len, rtsp_client);

	return ret;
}

int build_fua2_packet(unsigned char *inbuffer,int frame_size,int nalu_number,struct RtspClient* rtsp_client)
{
	unsigned char fua,fub;
	int i=1,len=0,time;
	RTP_header r;	   /**** 12 bytes ****/
	unsigned char nalu_buffer[NALU_BUFFER_SIZE];
	int seq = 0;
	int ret = 0;
	
	inbuffer+=4;
	build_rtp_header(&r,rtsp_client);
	time=((1000000/25)/nalu_number);
	
	while(i<=nalu_number){
		if(i==1){
			/**** FUA head packet ****/
			seq = rtsp_client_get_seq(rtsp_client);
			r.seq_no=htons(seq);
			rtsp_client_set_seq(rtsp_client, ++seq);
			memcpy(nalu_buffer,&r,sizeof(r));
			memcpy(nalu_buffer+13,inbuffer,SLICE_FSIZE);
			fua=nalu_buffer[12]=(inbuffer[0]& 0xE0)|28;
			fub=nalu_buffer[13]=0x80|(inbuffer[0]&0x1f);
			
			len=SLICE_SIZE;
			inbuffer+=SLICE_FSIZE;
			/****  write buff to client   ****/
			ret = udp_write(nalu_buffer, len,rtsp_client);
			if(ret < 0){
                break;
			}
		}
		else if(i==(nalu_number)){
			/**** FUA end packet ****/
			seq = rtsp_client_get_seq(rtsp_client);
			r.seq_no=htons(seq);
			rtsp_client_set_seq(rtsp_client, ++seq);
			r.marker=1;
			
			memcpy(nalu_buffer,&r,sizeof(r));
			memcpy(nalu_buffer+14,inbuffer,SLICE_FSIZE-1);
			nalu_buffer[12]=fua;
			nalu_buffer[13]=(fub&~0x80)|0x40;
			len=((frame_size-4)-((i-1)*(SLICE_FSIZE-1))-1+14);
			/****  write buff to client   ****/
			ret = udp_write(nalu_buffer, len,rtsp_client);
			if(ret < 0){
                break;
			}
		}
		i++;
	}

	return ret;
}

int build_fua3_packet(unsigned char *inbuffer,int frame_size,int nalu_number,struct RtspClient* rtsp_client)
{
	unsigned char fua,fub;
	int i=1,len=0,time;
	RTP_header r;	   /**** 12 bytes ****/
	unsigned char nalu_buffer[NALU_BUFFER_SIZE];
	int seq = 0;
    int ret = 0;	    

	inbuffer+=4;
	build_rtp_header(&r, rtsp_client);
	time=((1000000/25)/nalu_number);
	
	while(i<=nalu_number){
		if(i==1){
			/**** FUA head packet ****/
			seq = rtsp_client_get_seq(rtsp_client);
			r.seq_no=htons(seq);
			rtsp_client_set_seq(rtsp_client, ++seq);
			memcpy(nalu_buffer,&r,sizeof(r));
			memcpy(nalu_buffer+13,inbuffer,SLICE_FSIZE);
			fua=nalu_buffer[12]=(inbuffer[0]& 0xE0)|28;
			fub=nalu_buffer[13]=0x80|(inbuffer[0]&0x1f);
			
			len=SLICE_SIZE;
			inbuffer+=SLICE_FSIZE;
			
			/****  write buff to client   ****/
			ret = udp_write(nalu_buffer, len, rtsp_client);
			if(ret < 0){
                break;
			}
		}
		else if (i==nalu_number){
			/**** FUA end packet ****/
			seq = rtsp_client_get_seq(rtsp_client);
			r.seq_no=htons(seq);
			rtsp_client_set_seq(rtsp_client, ++seq);
			r.marker=1;
			
			memcpy(nalu_buffer,&r,sizeof(r));
			memcpy(nalu_buffer+14,inbuffer,SLICE_FSIZE-1);
			nalu_buffer[12]=fua;
			nalu_buffer[13]=(fub&~0x80)|0x40;
			//len=((frame_size-4)-((i-1)*(SLICE_FSIZE-1))+14);
			len=((frame_size-4)-((i-1)*(SLICE_FSIZE-1))-1+14);
			/****  write buff to client   ****/
			ret = udp_write(nalu_buffer, len, rtsp_client);
			if(ret < 0){
                break;
			}

		}
		else{
			/**** FUA middle packet ****/
			seq = rtsp_client_get_seq(rtsp_client);
			r.seq_no=htons(seq);
			rtsp_client_set_seq(rtsp_client, ++seq);
			memcpy(nalu_buffer,&r,sizeof(r));
			memcpy(nalu_buffer+14,inbuffer,(SLICE_FSIZE-1));
			nalu_buffer[12]=fua;
			nalu_buffer[13]=(fub&~0x80);
			len=SLICE_SIZE;
			inbuffer+=(SLICE_FSIZE-1);
			/****  write buff to client   ****/
			ret = udp_write(nalu_buffer, len,rtsp_client);
			if(ret < 0){
                break;
			}
		}
		i++;
	}
	
	return ret;
}

int send_nalu_packet(unsigned char *inbuffer,int frame_size,int nalu_number,struct RtspClient* rtsp_client)
{
    int ret = 0;
    
	switch(nalu_number){
		case 1:
			/****  singal nalu packet  ****/
			ret = build_signal_packet(inbuffer,frame_size,nalu_number,rtsp_client);
			break;
		case 2:
			/****  FUA header and end packet  ****/		
			ret = build_fua2_packet(inbuffer,frame_size,nalu_number,rtsp_client);
			break;
		default:
			/****  FUA header and middle end packet  ****/
			ret = build_fua3_packet(inbuffer,frame_size,nalu_number,rtsp_client);
			break;
	}
	
	return ret;
}

int build_i_packet(unsigned char *inbuffer,int frame_size,struct RtspClient* rtsp_client)
{
	int size=0;
	int i = 0;
	int ret = 0;

	if(frame_size <= SLICE_FSIZE){
		i=1;
		ret = send_nalu_packet(inbuffer, frame_size, i, rtsp_client);
		if(ret < 0){
	        return ret;	    
		}
	}
	else{
		/****  h264 nalu packet processing ****/
		i=(frame_size-4)/(SLICE_FSIZE-1);
		
		if(((frame_size-4)-(i*(SLICE_FSIZE-1)))>0){
			i+=1;
		}

		if(i <= 2){
			/**** only FUA start package and end  ****/
			ret = send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
            if(ret < 0){
                return ret;      
            }
		}
		else{
			/**** including FUA start  middle  end  package ****/
			ret = send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
            if(ret < 0){
                return ret;      
            }
		}
	}
	
	return size;	
}



int build_ps_packet(unsigned char *inbuffer,int frame_size,struct RtspClient* rtsp_client)
{
	int size=0,i=0;
	int ret = 0;
	
	if(!inbuffer){
		return -1;
	}

	size=(frame_size-4);
	if(frame_size <= SLICE_FSIZE){
		i=1;
		ret = send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
		if(ret < 0){
            return ret;
		}
	}
	else{
		printf("Waring: sps pps packet len lager SLICE_SIZE frame_size(%d)\n", frame_size);
	}

	return size;
}


int build_p_packet(unsigned char *inbuffer,int frame_size,struct RtspClient* rtsp_client)
{
	int size=0,i;
	int ret = 0;

	if(frame_size<=SLICE_FSIZE){
		i=1;
		send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
	}
	else{
		/****  h264 nalu packet processing ****/
		i=(frame_size-4)/(SLICE_FSIZE-1);
		
		if(((frame_size-4)-(i*(SLICE_FSIZE-1)))>0){
    		i+=1;
		}
		
		if(i<=2){
			/**** only FUA start package and end  ****/
			ret = send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
			if(ret < 0){
                return ret;
			}
		}
		else		{
			/**** including FUA start  middle  end  package ****/
			ret = send_nalu_packet(inbuffer,frame_size,i,rtsp_client);
			if(ret < 0){
                return ret;
			}
		}
	}

	return size;	
}

int parse_rtpnal(unsigned char *inbuffer,int frame_size,struct RtspClient* rtsp_client)
{
	int nalu_type=0;

	if(!inbuffer)	{
		return -1;
	}
	
	int ret = -1;
	
	nalu_type=(inbuffer[4]&0x1F);
	switch (nalu_type) 
	{
		case 6:
		case 7:
		case 8:
			/**** sps pps ****/
			ret = build_ps_packet(inbuffer,frame_size,rtsp_client);
			break;

		case 5:
			/**** I frame ****/
			ret = build_i_packet(inbuffer,frame_size,rtsp_client);
			break;
			
		case 1:
			/**** p frame ****/
			ret = build_p_packet(inbuffer,frame_size,rtsp_client);
			break;
		default:
			printf("not match rtp nalu type\n");
			break;
	}

	
	return ret;
}


int bulid_rtp_nalu(unsigned char *inbuffer,int frame_size,struct RtspClient* rtsp_client)
{
	int vlaue=0;
    int ret = -1;
	
	if(!inbuffer)	{
		return -1;
	}
	
	/****  check start code ****/
	vlaue=_check_start_code(inbuffer);
	if(vlaue){
		ret = parse_rtpnal(inbuffer,frame_size,rtsp_client);
	}

	return ret;
}

int get_h264_nalu(unsigned char *inbuffer, int inbuffer_data_len, unsigned char *outbuffer, struct RtspClient* rtsp_client)
{
	int frame_size=0;
	int ret = -1;

	return_val_if_fail(inbuffer != NULL && outbuffer != NULL, -1);
	
	frame_size = get_frame_size(inbuffer, inbuffer_data_len, outbuffer);
	
	ret = bulid_rtp_nalu(outbuffer, frame_size, rtsp_client);

    if(ret < 0){
        return -1;
    }

	
	return frame_size;
}

int rtp_send_data(struct RtspClient* rtsp_client,
                                unsigned char* data, 
                                int data_len, 
                                struct LRSDataHeader header)
{
    int frame_size = 0;
    unsigned char* p = NULL;
	unsigned char outbufs[READ_LEN]={0};
    int total_send_size = 0;
    int total_read_len = 0;
        
    p = &data[0];   
    total_read_len = data_len;
    
    for(;;){
        frame_size = get_h264_nalu(p, data_len, outbufs, rtsp_client);
        if(frame_size < 0){
            return -1;
        }
        total_send_size += frame_size;
        if(total_send_size >= total_read_len){
            break;
        }
        else{
            data_len -= frame_size;
            p += frame_size;
        }
    }
    
    return 0;
}

